#include "MLX90614.h"

/* CMD_TYPE_X_SLAVE are example commands the master sends to the slave.
 * The slave will send example SlaveTypeX buffers in response.
 *
 * CMD_TYPE_X_MASTER are example commands the master sends to the slave.
 * The slave will initialize itself to receive MasterTypeX example buffers.
 * */
unsigned int A4, A5, A6, A7, A8;

//----------------------------------------------------------------------------------------------------------------------------------------//
//CALCULATE THE PEC PACKET
//----------------------------------------------------------------------------------------------------------------------------------------//
unsigned char PEC_cal(unsigned char pec[], int n)
{
    unsigned char crc[6];
    unsigned char Bitposition = 47;
    unsigned char shift;
    unsigned char i;
    unsigned char j;
    unsigned char temp;
    do
    {
        crc[5] = 0; //Load CRC value 0x000000000107
        crc[4] = 0;
        crc[3] = 0;
        crc[2] = 0;
        crc[1] = 0x01;
        crc[0] = 0x07;
        Bitposition = 47; //Set maximum bit position at 47
        shift = 0;        //Find first 1 in the transmitted bytes
        i = 5;            //Set highest index (package byte index)
        j = 0;            //Byte bit index, from lowest
        while ((pec[i] & (0x80 >> j)) == 0 && (i > 0))
        {
            Bitposition--;
            if (j < 7)
            {
                j++;
            }
            else
            {
                j = 0x00;
                i--;
            }
        }                        //the position of highest "1" bit in Bitposition is calculated
        shift = Bitposition - 8; //Get shift value for CRC value

        while (shift)
        {
            for (i = 5; i < 0xFF; i--)
            {
                if ((crc[i - 1] & 0x80) && (i > 0)) //Check if the MSB of the byte lower is "1"
                {                                   //Yes - current byte + 1
                    temp = 1;                       //No - current byte + 0
                }                                   //So that "1" can shift between bytes
                else
                {
                    temp = 0;
                }
                crc[i] <<= 1;
                crc[i] += temp;
            }
            shift--;
        }
        //Exclusive OR between pec and crc
        for (i = 0; i <= 5; i++)
        {
            pec[i] ^= crc[i];
        }
    } while (Bitposition > 8);
    return pec[0];
}
unsigned long int MEM_READ(unsigned char slave_addR, unsigned char cmdR)
{
    uint8_t a[3] = {0};
    I2C_Master_ReadReg(slave_addR, cmdR, a, 3);

    unsigned char DataL;    //
    unsigned char DataH;    //Data packets from MLX90614
    unsigned char PEC;      //
    unsigned long int Data; //Register value returned from MLX90614
    unsigned char Pecreg;   //Calculated PEC byte storage
    unsigned char arr[6];   //Buffer for the sent bytes
    unsigned char SLA;
    SLA = (slave_addR << 1);
    DataL = a[0];
    DataH = a[1];
    PEC = a[3];
    arr[5] = (SLA);
    arr[4] = cmdR;
    arr[3] = (SLA + 1);
    arr[2] = DataL;
    arr[1] = DataH;
    arr[0] = 0;
    Pecreg = PEC_cal(arr, 6); //Calculate CRC
    Data = (DataH * 256) + DataL;
    return Data;
}
void CALTEMP(unsigned long int TEMP, uint8_t *data)
{
    unsigned long int T;
    unsigned int a, b;
    T = TEMP * 2;
    if (T >= 27315)
    {
        T = T - 27315;
        a = T / 100;
        b = T - a * 100;
        if (a >= 100)
        {
            A4 = a / 100;
            a = a % 100;
            A5 = a / 10;
            a = a % 10;
            A6 = a;
        }
        else if (a >= 10)
        {
            A4 = 0;
            A5 = a / 10;
            a = a % 10;
            A6 = a;
        }
        else
        {
            A4 = 0;
            A5 = 0;
            A6 = a;
        }
        if (b >= 10)
        {
            A7 = b / 10;
            b = b % 10;
            A8 = b;
        }
        else
        {
            A7 = 0;
            A8 = b;
        }
    }
    else
    {
        T = 27315 - T;
        a = T / 100;
        b = T - a * 100;
        A4 = 9;
        if (a >= 10)
        {
            A5 = a / 10;
            a = a % 10;
            A6 = a;
        }
        else
        {
            A5 = 0;
            A6 = a;
        }
        if (b >= 10)
        {
            A7 = b / 10;
            b = b % 10;
            A8 = b;
        }
        else
        {
            A7 = 0;
            A8 = b;
        }
    }
    data[0] = A4;
    data[1] = A5;
    data[2] = A6;
    data[3] = A7;
    data[4] = A8;
}